#include "MapViewer.h"
#include "MapPoint.h"
#include "KeyFrame.h"
#include "LevelHelpers.h"

#include <cvd/gl_helpers.h>

using namespace CVD;
using namespace std;


MapViewer::MapViewer(Map &map, GLWindow2 &glw):
mMap(map), mGLWindow(glw)
{
	mse3ViewerFromWorld = 
		SE3::exp((make_Vector, 0,0,2,0,0,0)) * SE3::exp((make_Vector, 0,0,0,0.8 * M_PI,0,0));
}

void MapViewer::DrawMapDots()
{
	SetupFrustum();
	SetupModelView();

	int nForMass = 0;
	glColor3f(0,1,1);
	glPointSize(3);
	glBegin(GL_POINTS);
	Zero(mv3MassCenter);
	for(size_t i=0; i<mMap.vpPoints.size(); i++)
	{
		Vector<3> v3Pos = mMap.vpPoints[i]->v3WorldPos;
		glColor(gavLevelColors[mMap.vpPoints[i]->nSourceLevel]);
		if(v3Pos * v3Pos < 10000)
		{
			nForMass++;
			mv3MassCenter += v3Pos;
		}
		glVertex(v3Pos);
	}
	glEnd();
	mv3MassCenter = mv3MassCenter / (0.1 + nForMass);
}


void MapViewer::DrawGrid()
{
	SetupFrustum();
	SetupModelView();
	glLineWidth(1);

	glBegin(GL_LINES);

	// Draw a larger grid around the outside..
	double dGridInterval = 0.1;

	double dMin = -100.0 * dGridInterval;
	double dMax =  100.0 * dGridInterval;

	for(int x=-10;x<=10;x+=1)
	{
		if(x==0)
			glColor3f(1,1,1);
		else
			glColor3f(0.3,0.3,0.3);
		glVertex3d((double)x * 10 * dGridInterval, dMin, 0.0);
		glVertex3d((double)x * 10 * dGridInterval, dMax, 0.0);
	}
	for(int y=-10;y<=10;y+=1)
	{
		if(y==0)
			glColor3f(1,1,1);
		else
			glColor3f(0.3,0.3,0.3);
		glVertex3d(dMin, (double)y * 10 *  dGridInterval, 0.0);
		glVertex3d(dMax, (double)y * 10 * dGridInterval, 0.0);
	}

	glEnd();

	glBegin(GL_LINES);
	dMin = -10.0 * dGridInterval;
	dMax =  10.0 * dGridInterval;

	for(int x=-10;x<=10;x++)
	{
		if(x==0)
			glColor3f(1,1,1);
		else
			glColor3f(0.5,0.5,0.5);

		glVertex3d((double)x * dGridInterval, dMin, 0.0);
		glVertex3d((double)x * dGridInterval, dMax, 0.0);
	}
	for(int y=-10;y<=10;y++)
	{
		if(y==0)
			glColor3f(1,1,1);
		else
			glColor3f(0.5,0.5,0.5);
		glVertex3d(dMin, (double)y * dGridInterval, 0.0);
		glVertex3d(dMax, (double)y * dGridInterval, 0.0);
	}

	glColor3f(1,0,0);
	glVertex3d(0,0,0);
	glVertex3d(1,0,0);
	glColor3f(0,1,0);
	glVertex3d(0,0,0);
	glVertex3d(0,1,0);
	glColor3f(1,1,1);
	glVertex3d(0,0,0);
	glVertex3d(0,0,1);
	glEnd();

	//   glColor3f(0.8,0.8,0.8);
	//   glRasterPos3f(1.1,0,0);
	//   mGLWindow.PrintString("x");
	//   glRasterPos3f(0,1.1,0);
	//   mGLWindow.PrintString("y");
	//   glRasterPos3f(0,0,1.1);
	//   mGLWindow.PrintString("z");
}

void MapViewer::DrawMap(SE3 se3CamFromWorld)
{
	mMessageForUser.str(""); // Wipe the user message clean

	// Update viewer position according to mouse input:
	{
		pair<Vector<6>, Vector<6> > pv6 = mGLWindow.GetMousePoseUpdate();
		SE3 se3CamFromMC;
		se3CamFromMC.get_translation() = mse3ViewerFromWorld * mv3MassCenter;
		mse3ViewerFromWorld = SE3::exp(pv6.first) * 
			se3CamFromMC * SE3::exp(pv6.second) * se3CamFromMC.inverse() * mse3ViewerFromWorld;
	}

	mGLWindow.SetupViewport();
	glClearColor(0,0,0,0);
	glClearDepth(1);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glEnable(GL_POINT_SMOOTH);
	glEnable(GL_LINE_SMOOTH);
	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	glColorMask(1,1,1,1);

	glEnable(GL_DEPTH_TEST);
	DrawGrid();
	DrawMapDots();
	DrawCamera(se3CamFromWorld);
	for(size_t i=0; i<mMap.vpKeyFrames.size(); i++)
		DrawCamera(mMap.vpKeyFrames[i]->se3CfromW, true);
	glDisable(GL_DEPTH_TEST);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	mMessageForUser << " Map: " << mMap.vpPoints.size() << "P, " << mMap.vpKeyFrames.size() << "KF";
	mMessageForUser << setprecision(4);
	mMessageForUser << "   Camera Pos: " << se3CamFromWorld.inverse().get_translation();
}

string MapViewer::GetMessageForUser()
{
	return mMessageForUser.str();
}

void MapViewer::SetupFrustum()
{
	glMatrixMode(GL_PROJECTION);  
	glLoadIdentity();
	double zNear = 0.03;
	glFrustum(-zNear, zNear, 0.75*zNear,-0.75*zNear,zNear,50);
	glScalef(1,1,-1);
	return;
};

void MapViewer::SetupModelView(SE3 se3WorldFromCurrent)
{
	glMatrixMode(GL_MODELVIEW);  
	glLoadIdentity();
	glMultMatrix(mse3ViewerFromWorld * se3WorldFromCurrent);
	return;
};

void MapViewer::DrawCamera(SE3 se3CfromW, bool bSmall)
{

	SetupModelView(se3CfromW.inverse());
	SetupFrustum();

	if(bSmall)
		glLineWidth(1);
	else
		glLineWidth(3);

	glBegin(GL_LINES);
	glColor3f(1,0,0);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.1f, 0.0f, 0.0f);
	glColor3f(0,1,0);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.1f, 0.0f);
	glColor3f(1,1,1);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.1f);
	glEnd();


	if(!bSmall)
	{
		glLineWidth(1);
		glColor3f(0.5,0.5,0.5);
		SetupModelView();
		Vector<2> v2CamPosXY = se3CfromW.inverse().get_translation().slice<0,2>();
		glBegin(GL_LINES);
		glColor3f(1,1,1);
		glVertex2d(v2CamPosXY[0] - 0.04, v2CamPosXY[1] + 0.04);
		glVertex2d(v2CamPosXY[0] + 0.04, v2CamPosXY[1] - 0.04);
		glVertex2d(v2CamPosXY[0] - 0.04, v2CamPosXY[1] - 0.04);
		glVertex2d(v2CamPosXY[0] + 0.04, v2CamPosXY[1] + 0.04);
		glEnd();
	}

}
